// 创建动态广播器节点
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.hpp>
#include <turtlesim_msgs/msg/pose.hpp>

using turtlesim_msgs::msg::Pose;

class TfBroadcasterNode : public rclcpp::Node {
public:
  // 构造函数
  TfBroadcasterNode() : Node("tf_broadcaster_node") {
    RCLCPP_INFO(this->get_logger(), "初始化动态广播器节点！");

    this->declare_parameter<std::string>("turtle", "turtle1");
    this->declare_parameter<std::string>("frame_id", "world");
    // 获取参数
    turtle_name = this->get_parameter("turtle").as_string();
    frame_id = this->get_parameter("frame_id").as_string();

    RCLCPP_INFO(this->get_logger(), "turtle_name: %s", turtle_name.c_str());
    RCLCPP_INFO(this->get_logger(), "frame_id: %s", frame_id.c_str());

    // 创建动态广播器对象
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
    // 创建乌龟位置姿态的订阅者
    subscription_ = this->create_subscription<Pose>(
        "/" + turtle_name + "/pose", 10,
        std::bind(&TfBroadcasterNode::topic_callback, this,
                  std::placeholders::_1));
  }
  // 发送动态变换 把乌龟的位置姿态发布出去 给 tf2
  void sendTransform(const geometry_msgs::msg::TransformStamped &t) {
    // 发送动态变换
    tf_broadcaster_->sendTransform(t);
    // RCLCPP_INFO(this->get_logger(), "发送动态变换成功！");
  }

private:
  // 乌龟名称
  std::string turtle_name;
  // 坐标系名称
  std::string frame_id;

  // 创建动态广播器对象的智能指针
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  // 创建乌龟位置姿态的订阅者
  rclcpp::Subscription<Pose>::SharedPtr subscription_;
  // 订阅回调函数
  void topic_callback(const Pose &msg) {

    // RCLCPP_INFO(this->get_logger(), "I heard: x: %.2f, y: %.2f, theta:
    // %.2f",msg.x, msg.y, msg.theta);

    // 创建并发布动态变换
    geometry_msgs::msg::TransformStamped t;
    // 设置时间戳
    t.header.stamp = this->get_clock()->now();
    // 地图坐标系
    t.header.frame_id = "world";

    // 父子坐标系
    t.child_frame_id = turtle_name;
    // 平移和旋转
    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;
    // 角速度换算四元数
    double theta = msg.theta; // 角度值
    // 计算四元数的值
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = sin(theta / 2);
    t.transform.rotation.w = cos(theta / 2);
    // 发布动态变换
    sendTransform(t);
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TfBroadcasterNode>());
  rclcpp::shutdown();
  return 0;
}
